
img = imread("demo.jpg");
% imshow(img)

points2D_Homo = ...
[405 210 1;
 522 199 1;
 457 665 1;
 559 639 1;
 959 320 1;
 943 541 1;
 1143 364 1;
 1113 616 1;
 512 651 1;
 952 383 1;
 634 695 1;
 677 683 1;
 896 914 1;
 946 889 1];

points3D_Homo = ...
[8 0 9 1;
 6 0 9 1;
 8 0 1 1;
 6 0 1 1;
 0 4 7 1;
 0 4 3 1;
 0 8 7 1;
 0 8 3 1;
 7 0 1 1;
 0 4 6 1;
 5 1 0 1;
 4 1 0 1;
 5 9 0 1;
 4 9 0 1];

[K, R, T,M, rho] = PinholeCameraCalibration(points2D_Homo, points3D_Homo);

% 重投影看像素点对应情况
points3D_Homo_test = points3D_Homo';
points2D_Homo_test = M * points3D_Homo_test;
points2D = [points2D_Homo_test(1,:) ./ points2D_Homo_test(3,:);points2D_Homo_test(2,:)./points2D_Homo_test(3,:)];

% K =
%    1.0e+03 *
%    1.229866653253700   0.000000004705684   0.734717308517352
%                    0   1.226720702852342   0.622741888452889
%                    0                   0   0.001000000000000

% R =
%   -0.850405660707642   0.526012363632373  -0.011009338866791
%   -0.224229311584156  -0.343423740339440   0.912020476961874
%    0.475953178435542   0.778055992764758   0.409996882988093

% T =
%   -1.753771891220250
%    0.098570476863861
%  -26.774395360428752

